#include "key.h"

Button board_button; // 板载key PB21
Button key1;         // PB24  U22

Button key2;         // PA22  U24
Button key3;         // PB20  U23
Button key4;         // PA25  U25
Button key5;         // PB25  U26

void map_key3_to_PA27(void)
{
    uint8_t state = DL_GPIO_readPins(KEY_KEY3_PORT, KEY_KEY3_PIN) ? 1 : 0;
    //uart0_printf("Key 3 state: %d\n", DL_GPIO_readPins(KEY_KEY3_PORT, KEY_KEY3_PIN));
    if (state == 0)
        DL_GPIO_clearPins(GW_GRAY_PORT, GW_GRAY_Key_PIN);
    else
        DL_GPIO_setPins(GW_GRAY_PORT, GW_GRAY_Key_PIN);
}

void btn0_single_click_handler(Button *btn)
{
    //uart0_printf("Button 0: Single Click\n");
    toggle_led_proc();
    if (menu_mode == MENU_PAGE)
    {
        menu_mode = 0;
    }
    else
    {
        //uart0_printf("page change! page: %d\n", menu_mode);
        menu_mode++;
    }
}

void btn1_single_click_handler(Button *btn) // U22
{
    // 增加量为0.1
    if (menu_mode == 1 && cursor_1 == 1)
    {
        PID_data_array[3][0] += 0.1f; // 增加Yaw_Kp
        uart0_printf("Yaw_kp ++ \n");
        uart0_printf("%.2f \n", PID_data_array[0][0]);
        uart0_printf("page: %d\n", menu_mode);
    }
    else if (menu_mode == 1 && cursor_1 == 2)
    {
        PID_data_array[3][1] += 0.1f; // 增加Yaw_Ki
    }
    else if (menu_mode == 1 && cursor_1 == 3)
    {
        PID_data_array[3][2] += 0.1f; // 增加Yaw_Kd
    }
    if (menu_mode == 1 && cursor_1 == 4)
    {
        // 增加GWkp
        PID_data_array[1][0] += 0.1f; // 增加GW_Kp
    }
    else if (menu_mode == 1 && cursor_1 == 5)
    {
        PID_data_array[1][1] += 0.1f; // 增加GW_Ki
    }
    else if (menu_mode == 1 && cursor_1 == 6)
    {
        PID_data_array[1][2] += 0.1f; // 增加GW_Kd
    }
    if (menu_mode == 2 && cursor_2 == 1)
    {
        // 增加servo1_angle
        servo1_angle += 10.0f; // 增加servo1_angle
        if (servo1_angle > 270.0f) servo1_angle = 270.0f; // 限制范围
    }
    else if (menu_mode == 2 && cursor_2 == 2)
    {
        // 增加servo3_angle
        servo3_angle += 10.0f; // 增加servo3_angle
        if (servo3_angle > 220.0f) servo3_angle = 220.0f; // 限制范围
    }
    else if (menu_mode == 2 && cursor_2 == 3)
    {
        // 增加servo3_angle
        servo1_angle += 0.1f; // 增加servo3_angle
        if (servo1_angle > 270.0f) servo1_angle = 270.0f; // 限制范围
    }
    else if (menu_mode == 2 && cursor_2 == 4)
    {
        // 增加servo1_xifen
        servo3_angle += 0.1f; // 增加servo1_xifen
        if (servo3_angle > 270.0f) servo3_angle = 270.0f; // 限制范围
    }
    if (menu_mode == 0 && cursor_0 == 4 )
    {
        N_round ++;
        if (N_round == 6) N_round = 5;

    }
    else if (menu_mode == 0 && cursor_0 == 3)
    {
        mission_num ++ ;
        if (mission_num == 4) mission_num = 3;
    }
    if (menu_mode == 1 && cursor_1 == 7)
    {
        //增加舵机P
        PID_data_array[2][0] += 0.01f; // 增加Servo_Kp

    }
    else if (menu_mode == 1 && cursor_1 == 8)
    {
        //增加舵机I
        PID_data_array[2][1] += 0.01f; // 增加Servo_Ki
    }
    else if (menu_mode == 1 && cursor_1 == 9)
    {
        //增加舵机D
        PID_data_array[2][2] += 0.01f; // 增加Servo_Kd
    }
    if (menu_mode == 0 && cursor_0 == 5)
    {
        //切换turnflag 当为0时左转电机 1时右转电机
        if (turn_flag == 0) turn_flag = 1;
        else turn_flag = 0;

    }

    /*     if (menu_mode == 1 && cursor_1 == 7)
        {
            //增加基础速度
            base_speed += 10.0f; // 增加基础速度
        }     */
    //uart0_printf("Button1 Single Click\n");
}

void btn2_single_click_handler(Button *btn) // U24
{
    // 减少量为0.1
    if (menu_mode == 1 && cursor_1 == 1)
    {
        PID_data_array[3][0] -= 0.1f; // 减少Yaw_Kp
    }
    else if (menu_mode == 1 && cursor_1 == 2)
    {
        PID_data_array[3][1] -= 0.1f; // 减少Yaw_Ki
    }
    else if (menu_mode == 1 && cursor_1 == 3)
    {
        PID_data_array[3][2] -= 0.1f; // 减少Yaw_Kd
    }
    if (menu_mode == 1 && cursor_1 == 4)
    {
        // 减少GWkp
        PID_data_array[1][0] -= 0.1f; // 减少GW_Kp
    }
    else if (menu_mode == 1 && cursor_1 == 5)
    {
        PID_data_array[1][1] -= 0.1f; // 减少GW_Ki
    }
    else if (menu_mode == 1 && cursor_1 == 6)
    {
        PID_data_array[1][2] -= 0.1f; // 减少GW_Kd
    }
    if (menu_mode == 2 && cursor_2 == 1)
    {
        // 减少servo1_angle
        servo1_angle -= 10.0f; // 减少servo1_angle
        if (servo1_angle < 0.0f) servo1_angle = 0.0f; // 限制范围
    }
    else if (menu_mode == 2 && cursor_2 == 2)
    {
        // 减少servo3_angle
        servo3_angle -= 10.0f; // 减少servo3_angle
        if (servo3_angle < 50.0f) servo3_angle = 50.0f; // 限制范围
    }
    else if (menu_mode == 2 && cursor_2 == 2)
    {
        // 减少servo3_angle
        servo3_angle -= 5.0f; // 减少servo3_angle
        if (servo3_angle < 50.0f) servo3_angle = 50.0f; // 限制范围
    }
    if (menu_mode == 2 && cursor_2 == 3)
    {
        // 减少servo1_xifen
        servo1_angle -= 0.1f; // 减少servo1_xifen
        if (servo1_angle < 0.0f) servo1_angle = 0.0f; // 限制范围
    }
    else if (menu_mode == 2 && cursor_2 == 4)
    {
        // 减少servo3_xifen
        servo3_angle -= 0.1f; // 减少servo3_xifen
        if (servo3_angle < 50.0f) servo3_angle = 50.0f; // 限制范围
    }
    if (menu_mode == 0 && cursor_0 == 4) 
    {
        N_round--;
        if (N_round < 1) N_round = 1;
    }
    else if (menu_mode == 0 && cursor_0 == 3 )
    {
        mission_num --;
        if (mission_num == 0) mission_num = 1;

    }
    if (menu_mode == 1 && cursor_1 == 7)
    {
        //减少舵机P
        PID_data_array[2][0] -= 0.01f; // 减少Servo_Kp
    }
    else if (menu_mode == 1 && cursor_1 == 8)
    {
        //减少舵机I
        PID_data_array[2][1] -= 0.01f; // 减少Servo_Ki
    }
    else if (menu_mode == 1 && cursor_1 == 9)
    {
        //减少舵机D
        PID_data_array[2][2] -= 0.01f; // 减少Servo_Kd
    }
    
    /*     if (menu_mode == 1 && cursor_1 == 7)
        {
            //减少基础速度
            base_speed -= 10.0f; // 减少基础速度
        } */
    //uart0_printf("Button Single Click\n");
}

void btn3_single_click_handler(Button *btn) // U23
{
    uart0_printf("Button Single Click\n");
    if (menu_mode == 3) // 按键映射到PA27
    {

        return;
    }
}

void btn4_single_click_handler(Button *btn) // U25
{
    if (Mission_Start_Flag == 0 && menu_mode == 0)
    {
        Mission_Start_Flag = 1; // 题目开始运行
        Encoder_Start = Encoder_Now[0] + Encoder_Now[3]; // 记录起始编码器值
        Encoder_End = Encoder_Start + distance_to_encoder(400 * 2 * N_round - 13 * 2 * N_round); // 计算结束编码器值
    }
}

void btn5_single_click_handler(Button *btn)
{
    if (menu_mode == 0)
    {
        cursor_0++;
        if (cursor_0 == MENU0_CURSORMAX)
            cursor_0 = 3; // 循环
    }
    else if (menu_mode == 1)
    {
        cursor_1++;
        if (cursor_1 >= MENU1_CURSORMAX)
            cursor_1 = 1; // 循环
    }
    else if (menu_mode == 2)
    {
        cursor_2++;
        if (cursor_2 >= MENU2_CURSORMAX)
            cursor_2 = 1; // 循环
    }
    uart0_printf("Button Single Click\n");
}

void btn5_double_click_handler(Button *btn)
{
    EEPROM_Save();
    buzzer_on();
    uart0_printf("Button double Click\n");
    //    uart0_printf("Write: %.2f\n", PID_data_array[3][0]);
    //    uart0_printf("Read: %.2f\n",EEPROM_ReadFloat(YAW_KP_ADDR));
}

void key_init(void)
{
    // Initialize board button
    button_init(&board_button, read_button_gpio, 0, 0); // Pb21 板载按键
    button_attach(&board_button, BTN_SINGLE_CLICK, btn0_single_click_handler);
    button_start(&board_button);

    // Initialize other buttons
    button_init(&key1, read_button_gpio, 0, 1); // Pb24
    button_attach(&key1, BTN_SINGLE_CLICK, btn1_single_click_handler);
    button_start(&key1);

    button_init(&key2, read_button_gpio, 0, 2); // PA22
    button_attach(&key2, BTN_SINGLE_CLICK, btn2_single_click_handler);
    button_start(&key2);

    button_init(&key3, read_button_gpio, 0, 3); // PB20
    button_attach(&key3, BTN_SINGLE_CLICK, btn3_single_click_handler);
    button_start(&key3);

    button_init(&key4, read_button_gpio, 0, 4); // PA25
    button_attach(&key4, BTN_SINGLE_CLICK, btn4_single_click_handler);
    button_start(&key4);

    button_init(&key5, read_button_gpio, 0, 5); // PB25
    button_attach(&key5, BTN_SINGLE_CLICK, btn5_single_click_handler);
    button_attach(&key5, BTN_DOUBLE_CLICK, btn5_double_click_handler);
    button_start(&key5);
}

void key_proc(void)
{
}